% 惯性系对准误差仿真
%
% References:
% [1] 理论文档“惯性系对准姿态误差”

clc;
clear;

load('earthconstants_wgs84', 'omegaIE');
g = 9.8;
L = pi/6;
fL = [0 0 -g]';
omegaIEL = [omegaIE*cos(L) 0 -omegaIE*sin(L)]';
dt = 0.005;
simNum = 6000;

phiE_I0L_Theory = NaN(simNum, 3);
phiE_I0L = NaN(simNum, 3);
dOrth1 = NaN(simNum, 3);
dNorm1 = NaN(simNum, 3);
phiB_I0L_Theory = NaN(simNum, 3);
phiB_I0L = NaN(simNum, 3);
dOrth2 = NaN(simNum, 3);
dNorm2 = NaN(simNum, 3);
CB_I0E_I0dvSFB_I0_theory = NaN(simNum, 3);
CB_I0E_I0dvSFB_I0_actual = NaN(simNum, 3);
phiL_Theory = NaN(simNum, 3);
phiL = NaN(simNum, 3);
dfB = [90 90 90]' * 1e-6 * g; % 转换前单位ug
domegaIBB = [0.012 0.0072 0.036]' * pi / 180 / 3600; % 转换前单位°/h
% dfB = [0 0 0]' * 1e-6 * g; % 转换前单位ug
% domegaIBB = [0.0 0.0 0.0]' * pi / 180 / 3600; % 转换前单位°/h
CBL = angle2dcm(pi, 0, 0, 'ZYX');
CLN = [0 1 0; 1 0 0; 0 0 -1];
rvB = CBL*omegaIEL;
uZEE_I0 = [0 0 1]';

%% 初始化
clear('inertcoarsealign_stat');
in_ICAS.specVelInc = NaN(3, 1);
in_ICAS.angleInc = NaN(3, 1);
in_ICAS.t = 0;
par_ICAS.samplePeriod = dt;
par_ICAS.t1 = simNum*dt - 5;
par_ICAS.L = L;
par_ICAS.g = g;
par_ICAS.bufLen = int32(7000);
par_ICAS.bufInterval = int32(60);
doPolyFit = false;
inertcoarsealign_stat(int8(0), in_ICAS, par_ICAS, omegaIE, doPolyFit);

%% 解算周期
out_ICAS.cycCnt = NaN(simNum, 1);
out_ICAS.CBL = NaN(3, 3, simNum);
out_ICAS.azm = NaN(simNum, 3);
auxOut_ICAS.CB_I0E_I0 = NaN(3, 3, simNum);
auxOut_ICAS.CBB_I0 = NaN(3, 3, simNum);
auxOut_ICAS.vSFB_I0 = NaN(simNum, 3);
CBB_I0_true = NaN(3, 3, simNum);
CB_I0E_I0_true = NaN(3, 3, simNum);
vSFB_I0_true = NaN(simNum, 3);
for i=1:simNum
    t = i*dt;
    % 计算值
    fhatB = CBL * fL + dfB;
    omegahatIBB = CBL' * omegaIEL + domegaIBB;
    in_ICAS.specVelInc = fhatB * dt;
    in_ICAS.angleInc = omegahatIBB * dt;
    in_ICAS.t = i*0.005;
    
    [tmp_out, tmp_auxOut] = inertcoarsealign_stat(int8(1), in_ICAS, par_ICAS, omegaIE, doPolyFit);
    out_ICAS.cycCnt(i, 1) = tmp_out.cycCnt;
    out_ICAS.CBL(:, :, i) = tmp_out.CBL;
    out_ICAS.azm(i, :) = bodyaxisazimuth(tmp_out.CBL);
    auxOut_ICAS.CB_I0E_I0(:, :, i) = tmp_auxOut.CB_I0E_I0;
    auxOut_ICAS.CBB_I0(:, :, i) = tmp_auxOut.CBB_I0;
    auxOut_ICAS.vSFB_I0(i, :) = tmp_auxOut.vSFB_I0';
    
    % 真值
    % 1. CBB_I0
    CBB_I0_true(:, :, i) = rv2dcm_exact(rvB*t);
    % 2. CB_I0E_I0
    CE_I0N = dcmile2enu(t, L, omegaIE, 0);
    CB_I0E_I0_true(:, :, i) = (CBB_I0_true(:, :, i) * ((CLN * CBL)' ) * CE_I0N)';
    % 3. vSFB_I0
    fB = CBL * fL;
    uZEB_I0 = CB_I0E_I0_true(:, :, i)' * uZEE_I0;
    vSFB_I0_true(i, :) = t*(eye(3)+(1-cos(omegaIE*t))/(omegaIE*t)*skewsymmat(uZEB_I0) ...
        +(1-sin(omegaIE*t)/(omegaIE*t))*skewsymmat(uZEB_I0)*skewsymmat(uZEB_I0)) ...
        * fB;
    
    % 理论误差
    dfL = CBL * dfB;
    dfN = CLN * dfL;
    domegaIBL = CBL * domegaIBB;
    domegaIBN = CLN * domegaIBL;
    CB_I0E_I0dvSFB_I0_theory(i, 1) = -(dfN(1)*omegaIE*t^2)/2 - dfN(2)*t*sin(L) + dfN(3)*t*cos(L) + (domegaIBN(1)*g*t^2*sin(L))/2 - (domegaIBN(2)*g*omegaIE*t^3)/3;
    CB_I0E_I0dvSFB_I0_theory(i, 2) = dfN(1)*t - (dfN(2)*omegaIE*t^2*sin(L))/2 + (dfN(3)*omegaIE*t^2*cos(L))/2 + (domegaIBN(1)*g*omegaIE*t^3*sin(L))/3 + (domegaIBN(2)*g*t^2)/2;
    CB_I0E_I0dvSFB_I0_theory(i, 3) = dfN(2)*t*cos(L) + dfN(3)*t*sin(L) - (domegaIBN(1)*g*t^2*cos(L))/2;
    if t > 1
        t1 = 1;
        if t > par_ICAS.t1
            t1 = par_ICAS.t1;
        end
        t2 = t;
        phiE_I0L_Theory(i, 1) = dfL(2)/g - domegaIBL(2)*omegaIE*t1*t2*sin(L)/6 + domegaIBL(1)*(t1+t2)/3;
        phiE_I0L_Theory(i, 2) = -dfL(1)/g + domegaIBL(1)*omegaIE*t1*t2*sin(L)/3;
        phiE_I0L_Theory(i, 3) = -dfL(2)*tan(L)/g + domegaIBL(2)/omegaIE/cos(L) - 2*domegaIBL(1)*(t1+t2)*tan(L)/3;
    end
    phiB_I0L_Theory(i, 1) = -domegaIBL(1)*t-domegaIBL(2)*omegaIE*sin(L)*t^2/2;
    phiB_I0L_Theory(i, 2) = -domegaIBL(2)*t+domegaIBL(1)*omegaIE*sin(L)*t^2/2+domegaIBL(3)*omegaIE*cos(L)*t^2/2;
    phiB_I0L_Theory(i, 3) = -domegaIBL(3)*t-domegaIBL(2)*omegaIE*cos(L)*t^2/2;
    phiL_Theory(i, :) = phiE_I0L_Theory(i, :) + phiB_I0L_Theory(i, :);
    
    % 实际误差
    CB_I0E_I0dvSFB_I0_actual(i, :) = (CB_I0E_I0_true(:, :, i)*(tmp_auxOut.vSFB_I0-vSFB_I0_true(i, :)'))';
    [phiE_I0, dOrth1(i, :), dNorm1(i, :)] = dcmdiff(tmp_auxOut.CB_I0E_I0, CB_I0E_I0_true(:, :, i));
    CE_I0L = (CLN)' * CE_I0N;
    phiE_I0L(i, :) = (CE_I0L * phiE_I0')';
    [phiB_I0L(i, :), dOrth2(i, :), dNorm2(i, :)] = dcmdiff(tmp_auxOut.CBB_I0, CBB_I0_true(:, :, i));
    [phiL(i, :)] = dcmdiff(tmp_out.CBL, CBL);
end
% 误差比较
t = (1:simNum)*0.005;
preparefig('phiB_I0L');
subplot(2, 2, 1);
plot(t, phiB_I0L/pi*180*3600); xlabel('t/s'); ylabel('实际误差/″');
subplot(2, 2, 2);
plot(t, phiB_I0L_Theory/pi*180*3600); xlabel('t/s'); ylabel('理论误差/″');
subplot(2, 2, [3, 4]);
plot(t, (phiB_I0L-phiB_I0L_Theory)/pi*180*3600); xlabel('t/s'); ylabel('实际误差与理论误差的差值/″');

preparefig('CB_I0E_I0dvSFB_I0');
subplot(2, 2, 1);
plot(t, CB_I0E_I0dvSFB_I0_actual); xlabel('t/s'); ylabel('实际误差/(m/s)');
subplot(2, 2, 2);
plot(t, CB_I0E_I0dvSFB_I0_theory); xlabel('t/s'); ylabel('理论误差/(m/s)');
subplot(2, 2, [3, 4]);
plot(t, (CB_I0E_I0dvSFB_I0_actual-CB_I0E_I0dvSFB_I0_theory)); xlabel('t/s'); ylabel('实际误差与理论误差的差值/(m/s)');

preparefig('phiE_I0L');
subplot(2, 2, 1);
plot(t, phiE_I0L/pi*180*3600); xlabel('t/s'); ylabel('实际误差/″');
subplot(2, 2, 2);
plot(t, phiE_I0L_Theory/pi*180*3600); xlabel('t/s'); ylabel('理论误差/″');
subplot(2, 2, [3, 4]);
plot(t, (phiE_I0L-phiE_I0L_Theory)/pi*180*3600); xlabel('t/s'); ylabel('实际误差与理论误差的差值/″');

preparefig('phiL');
subplot(2, 2, 1);
plot(t, phiE_I0L/pi*180*3600); xlabel('t/s'); ylabel('实际误差/″');
subplot(2, 2, 2);
plot(t, phiL_Theory/pi*180*3600); xlabel('t/s'); ylabel('理论误差/″');
subplot(2, 2, [3, 4]);
plot(t, (phiE_I0L-phiL_Theory)/pi*180*3600); xlabel('t/s'); ylabel('实际误差与理论误差的差值/″');

